#include "small_driver_uart_control.h"
#include "small_driver_uart_control.h"
#include "motor_driver.h"
#include "imu_driver.h"
#include "car.h"
struct brushless_motor motor;


void brushless_motor_init(struct brushless_motor *self)
{
    small_driver_uart_init();
    self ->direction = reverse_turn;
    self->order = postive;
    self ->speed_left = 0;
    self ->speed_right = 0;
    self ->torque_left = 0;
    self ->torque_right = 0;
    self->real_delta_m = 0;
}

void brushless_motor_get_speed(struct brushless_motor *self)

{

    if (self->order == postive)
        {
            if(self ->direction == turn_turn)
            {
                self->speed_left =motor_value.receive_left_speed_data;
                self->speed_right =motor_value.receive_right_speed_data;
            }
            else if(self ->direction == turn_reverse)
            {
                self->speed_left =-motor_value.receive_left_speed_data;
                self->speed_right =motor_value.receive_right_speed_data;
            }
            else if(self ->direction == reverse_turn)
            {
                self->speed_left =motor_value.receive_left_speed_data;
                self->speed_right =-motor_value.receive_right_speed_data;
            }
            else
            {
                self->speed_left =-motor_value.receive_left_speed_data;
                self->speed_right =-motor_value.receive_right_speed_data;
            }
        }
    else
        {
            if(self ->direction == turn_turn)
            {
                self->speed_left =motor_value.receive_right_speed_data;
                self->speed_right =motor_value.receive_left_speed_data;
            }
            else if(self ->direction == turn_reverse)
            {
                self->speed_left =motor_value.receive_right_speed_data;
                self->speed_right =-motor_value.receive_left_speed_data;
            }
            else if(self ->direction == reverse_turn)
            {
                self->speed_left =-motor_value.receive_right_speed_data;
                self->speed_right =motor_value.receive_left_speed_data;
            }
            else
            {
                self->speed_left =-motor_value.receive_right_speed_data;
                self->speed_right =-motor_value.receive_left_speed_data;
            }
        }
    //self->average_v = (self->speed_left+self->speed_right)/2;


    self->average_v = (self->speed_left + self->speed_right)/2;


    self->real_delta_m = self->average_v/60*0.005*0.21;

    //self->speed_left += imu_0.v_angle.pitch*33.333333333333333f;
    //self->speed_right += imu_0.v_angle.pitch*33.333333333333333f;
}



void brushless_motor_set_torque(struct brushless_motor *self,int16 left_torque, int16 right_torque)
{



    if (self->order == postive)
    {
            self ->torque_left = left_torque;
            self ->torque_right = right_torque;
            if(self ->direction == turn_turn)
            small_driver_set_duty(self ->torque_left,self ->torque_right);
            else if(self ->direction == turn_reverse)
            small_driver_set_duty(self ->torque_left,-self ->torque_right);
            else if(self ->direction == reverse_turn)
            small_driver_set_duty(-self ->torque_left,self ->torque_right);
            else
            small_driver_set_duty(-self ->torque_left,-self ->torque_right);

    }
    else
    {
            self ->torque_left = left_torque;
            self ->torque_right = right_torque;
            if(self ->direction == turn_turn)
                small_driver_set_duty(self ->torque_right,self ->torque_left);
            else if(self ->direction == turn_reverse)
                small_driver_set_duty(self ->torque_right,-self ->torque_left);
            else if(self ->direction == reverse_turn)
                small_driver_set_duty(-self ->torque_right,self ->torque_left);
            else
                small_driver_set_duty(-self ->torque_right,-self ->torque_left);

    }

}
